/*LICENSE_BEGIN
 * ******************************************************************
 * Copyright 2006 by the Dainamite Robocup-Team at the
 * DAI-Labor, TU-Berlin (www.dai-labor.de)
 * 
 * Members of Dainamite are:
 * Holger Endert
 * Robert Wetzker
 * Thomas Karbe
 * Axel Heßler
 * Philippe Bütter
 * Felix Brossmann
 * 
 * Licensed under the GNU GPL (General Public License) Free Software 
 * License, Version 2. You may use this file only in compliance with 
 * the License. You may obtain a copy of the License at:
 * 
 * http://www.fsf.org/licensing/licenses/gpl.txt
 * ******************************************************************
LICENSE_END*/
package robocup.component.worldmodel.particlefilter;

import java.util.Random;

import robocup.component.SConf;
import robocup.component.geometry.Vektor;
import robocup.component.infotypes.BallInfo;

/**
 * Particle filter used for improving the balls position and speed.
 * 
 * (based on the UvA-Trilearn Master Thesis)
 * 
 * @author werwurm
 *
 */
public class BallParticleFilter {

	private Vektor particlesPos[];

	private Vektor particlesSpeed[];

	private Vektor particlesPos2[];

	private static final int particleCount = 300;

	private boolean speedInitialized = false;

	private boolean ballKicked = true;

	private int validParticles = 0;

	private Random random = new Random(System.currentTimeMillis());

	private BallRegion posRegionHelper = new BallRegion();

	private BallSpeedRegion speedRegionHelper = new BallSpeedRegion();

	private Vektor vektorHelper = new Vektor();

	private int translationSinceLastVI = 0;

	/**
	 * This class is a particle-filter for estimating the balls position and
	 * velocity.
	 */
	public BallParticleFilter() {
		super();
		this.particlesPos = new Vektor[particleCount];
		this.particlesSpeed = new Vektor[particleCount];
		this.particlesPos2 = new Vektor[particleCount];
		for (int i = 0; i < particleCount; i++) {
			this.particlesPos[i] = new Vektor();
			this.particlesSpeed[i] = new Vektor();
			this.particlesPos2[i] = new Vektor();
		}
		this.validParticles = 1;
		this.particlesPos[0].x = 0;
		this.particlesPos[0].y = 0;
		this.particlesSpeed[0].x = 0;
		this.particlesSpeed[0].y = 0;

	}

	public void resetPosition(Vektor pos, Vektor speed) {
		if (pos == null) {
			this.validParticles = 0;
		} else {
			this.validParticles = 1;
			this.particlesPos[0].x = pos.x;
			this.particlesPos[0].y = pos.y;
			this.particlesSpeed[0].x = speed.x;
			this.particlesSpeed[0].y = speed.y;

		}
		this.translationSinceLastVI = 0;
	}

	public void addVI(BallInfo bi, double headAngle, Vektor myPos,
			Vektor mySpeed) {
		boolean speedSeen = bi.velocitySeen();
		this.posRegionHelper.reinitRegionPos(bi, headAngle, myPos);
		if (speedSeen) {
			this.speedRegionHelper.reinitRegion(bi, headAngle,
					this.posRegionHelper, mySpeed);
		}

		Vektor swaphelper;
		if (this.validParticles != 0) {
			this.ballKicked = false;
			if (!this.speedInitialized) {
				if (speedSeen) {
					for (int i = 0; i < this.validParticles; i++) {
						this.speedRegionHelper
								.makeRandomInsideRegion(this.particlesSpeed[i]);
					}
					this.speedInitialized = true;
				} else if (this.translationSinceLastVI == 1) {
					for (int i = 0; i < this.particlesPos2.length; i++) {
						this.posRegionHelper
								.makeRandomInsideRegion(this.particlesPos2[i]);
						Vektor chosen = this.particlesPos[this.random
								.nextInt(this.validParticles)];
						this.particlesSpeed[i].x = this.particlesPos2[i].x;
						this.particlesSpeed[i].y = this.particlesPos2[i].y;
						this.particlesSpeed[i].subFromThis(chosen);
					}
					Vektor swapHelper[] = this.particlesPos;
					this.particlesPos = this.particlesPos2;
					this.particlesPos2 = swapHelper;
					this.validParticles = this.particlesPos.length;
				}
				this.speedInitialized = true;
			}
			int lastValidPlusOne = 0;
			for (int i = 0; i < this.validParticles; i++) {
				if (this.posRegionHelper.insideRegion(this.particlesPos[i]) == 1
						&& (!this.speedInitialized || !speedSeen || this.speedRegionHelper
								.insideRegion(this.particlesSpeed[i]) == 1)) {
					swaphelper = this.particlesPos[i];
					this.particlesPos[i] = this.particlesPos[lastValidPlusOne];
					this.particlesPos[lastValidPlusOne] = swaphelper;
					swaphelper = this.particlesSpeed[i];
					this.particlesSpeed[i] = this.particlesSpeed[lastValidPlusOne];
					this.particlesSpeed[lastValidPlusOne] = swaphelper;
					lastValidPlusOne++;
				}
			}
			this.validParticles = lastValidPlusOne;
			if (lastValidPlusOne != 0) {
				// clone valid Particles
				for (int i = lastValidPlusOne; i < this.particlesPos.length; i++) {
					int chosen = this.random.nextInt(lastValidPlusOne);
					this.particlesPos[i].x = this.particlesPos[chosen].x;
					this.particlesPos[i].y = this.particlesPos[chosen].y;
					this.particlesSpeed[i].x = this.particlesSpeed[chosen].x;
					this.particlesSpeed[i].y = this.particlesSpeed[chosen].y;
				}
				this.validParticles = this.particlesPos.length;
			}
		}

		if (this.validParticles == 0) {
			this.ballKicked = true;
			for (int i = 0; i < this.particlesPos.length; i++) {
				this.posRegionHelper
						.makeRandomInsideRegion(this.particlesPos[i]);
				if (speedSeen) {
					this.speedRegionHelper
							.makeRandomInsideRegion(this.particlesSpeed[i]);
				}
			}
			this.speedInitialized = speedSeen;
			this.validParticles = this.particlesPos.length;
		}
		this.translationSinceLastVI = 0;
	}

	public boolean isSpeedInitialized() {
		return this.speedInitialized;
	}

	public Vektor getPosition() {
		if (this.validParticles == 0) {
			return null;
		}
		Vektor mean = this.vektorHelper;
		mean.x = 0;
		mean.y = 0;
		for (int i = 0; i < this.validParticles; i++) {
			mean.x += this.particlesPos[i].x;
			mean.y += this.particlesPos[i].y;
		}
		mean.x /= (double) this.validParticles;
		mean.y /= (double) this.validParticles;
		return mean.cloned();
	}

	public Vektor getSpeed() {
		if (this.validParticles == 0 || !this.speedInitialized) {
			return null;
		}
		Vektor mean = this.vektorHelper;
		mean.x = 0;
		mean.y = 0;
		for (int i = 0; i < this.validParticles; i++) {
			mean.x += this.particlesSpeed[i].x;
			mean.y += this.particlesSpeed[i].y;
		}
		mean.x /= (double) this.validParticles;
		mean.y /= (double) this.validParticles;
		return mean.cloned();

	}

	public boolean isBallKicked() {
		return this.ballKicked;
	}

	public void translatePosition() {
		this.translationSinceLastVI++;
		if (!this.speedInitialized)
			return;
		for (int i = 0; i < this.validParticles; i++) {
			double rmax = SConf.getInstance().BALL_RAND
					* this.particlesSpeed[i].getLength();
			this.vektorHelper.x = this.particlesSpeed[i].x
					+ ((this.random.nextDouble() * 2.0) - 1.0) * rmax;
			this.vektorHelper.y = this.particlesSpeed[i].y
					+ ((this.random.nextDouble() * 2.0) - 1.0) * rmax;
			this.particlesPos[i].addToThis(this.vektorHelper);
			this.particlesSpeed[i].x = this.vektorHelper.x
					* SConf.getInstance().BALL_DECAY;
			this.particlesSpeed[i].y = this.vektorHelper.y
					* SConf.getInstance().BALL_DECAY;
		}
	}
}
